;/*=============================================================================
;#    The element-comm is a tentative program used to test our ideas.           
;#                       Copyright (C)  bgao                                    
;#
;#     This program is free software; you can redistribute it and/or            
;#      modify it under the terms of the GNU General Public License             
;#     as published by the Free Software Foundation; either version 2           
;#         of the License, or (at your option) any later version.               
;#
;#     This program is distributed in the hope that it will be useful,          
;#    but WITHOUT ANY WARRANTY; without even the implied warranty of           
;#     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the            
;#             GNU General Public License for more details.                    
;#
;#    You should have received a copy of the GNU General Public License        
;#     along with this program; if not, write to the Free Software             
;#      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,             
;#                       MA  02110-1301, USA.                                  
;#==============================================================================
;# $Id$ 
;#
;# $Author$ 
;#
;# $Revision$ 
;#
;# $Date$ 
;#
;# Description: 
;#
;=============================================================================*/
#include <avr/io.h>

.global wsn_context_switch

wsn_context_switch:
	/* CONTEXT SAVE */

 	/* save general registers into stack */
	
 	PUSH R2
	PUSH R3
	PUSH R4
	PUSH R5
	PUSH R6
	PUSH R7
	PUSH R8
	PUSH R9
	PUSH R10
	PUSH R11
	PUSH R12
	PUSH R13
	PUSH R14
	PUSH R15
	PUSH R16
	PUSH R17
	PUSH R28
	PUSH R29

	
	/* save SP */ /*R25 R24 is the old thread context address = parameter1*/
	MOV R27, R25
	MOV R26, R24
	
	
	IN	R16, _SFR_IO_ADDR(SPH)
	IN	R17, _SFR_IO_ADDR(SPL)
	ST	X+, R16
	ST	X+, R17

	/* CONTEXT RESTORE *//*R23 R22 is the new thread context address = parameter2*/
	CLR R1
	MOV R27, R23
	MOV R26, R22

	LD R16, X+
	LD R17, X+
	OUT _SFR_IO_ADDR(SPH), R16
	OUT _SFR_IO_ADDR(SPL), R17

	POP R29
	POP R28
	POP R17
	POP R16
	POP R15
	POP R14
	POP R13
	POP R12
	POP R11
	POP R10
	POP R9
	POP R8
	POP R7
	POP R6
	POP R5
	POP R4
	POP R3
	POP R2
	RET
